
#!/usr/bin/python3
# coding=utf8
import sys
sys.path.append('/home/pi/MasterPi/')
import RPi.GPIO as GPIO
import serial
import threading
import time
import ctypes
import math
from controller import CMD_TAKE_OFF,CMD_STOP,CMD_ALL_ANGLE,CMD_SERVO_FORWARD,CMD_SERVO_BACK

CMD_LEN = 9
RECV_LEN = 32
IBUS_LEN = 32

class MySerial:
    def __init__(self,controller):
        self.__controller = controller
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BOARD)
        GPIO.setup(7, GPIO.OUT)
        GPIO.output(7, 0)
        self.__ser = serial.Serial(port='/dev/ttyAMA0',          # 端口名称，根据你的系统修改为正确的端口
                                    baudrate=115200,        # 波特率
                                    bytesize=serial.EIGHTBITS,  # 数据位
                                    parity=serial.PARITY_NONE,   # 奇校验
                                    stopbits=serial.STOPBITS_ONE  # 停止位
                                   )
    
 





    # Function to receive data
    def receive_data(self):
        isStarted = False
        recvCounts = 0
        cmdBytes = [i for i in range(CMD_LEN)]
        oneframe =[i for i in range(IBUS_LEN)]
        recvIndex = 2
        flagFrame = 0
        while True:
            data = self.__ser.read(RECV_LEN)  # Read data from the serial port
   
            if data:
                for i in range(len(data)):
                    onebyte = data[i]
                    if onebyte == 0x20:
                        flagFrame = 1
                        oneframe[0]=onebyte
                    elif onebyte == 0x40 and flagFrame == 1:
                        flagFrame = 2
                        oneframe[1]=onebyte
                    elif flagFrame == 2:
                        oneframe[recvIndex] = onebyte
                        recvIndex +=1
                        if recvIndex == IBUS_LEN:
                            # ok oneframe ont checked
                             
                            parsed = self.parse_ibus_32byte_frame(oneframe)
                            recvIndex = 2
                            flagFrame = 0
                            if parsed['valid'] == True:
                                #print(parsed['channels'][5]) # 0:yaw ,4:arm ,1:throlle
                                arm = parsed['channels'][4]
                                servo_direction = parsed['channels'][5]
                                yaw = parsed['channels'][0] - 1500
                                throlle = parsed['channels'][1] - 1000
                                if throlle < 0:
                                    throlle = 0
                                
                                if servo_direction < 1500:
                                    #forward
                                    self.__controller.exec(CMD_SERVO_FORWARD,[])
                                else:
                                    #backword
                                    self.__controller.exec(CMD_SERVO_BACK,[])
                                 
                                if arm > 1500:
                                    #arm
                                    self.__controller.exec(CMD_TAKE_OFF,[])
                                else:
                                    #disarm
                                    self.__controller.exec(CMD_STOP,[])
                                    return
                                
                                
                                self.__controller.exec(CMD_ALL_ANGLE,[throlle/10,yaw/20])
                                
                                #print(throlle/10,yaw/20)
                                
                

                
    def ibus_checksum(self,data: bytes) -> int:
        checksum = 0xFFFF - sum(data)
        return checksum & 0xFFFF  # 确保返回 16 位

    def is_valid_ibus_frame(self,frame: bytes) -> bool:
        if len(frame) < 4:
            return False
        
        # 检查帧头（0x20 0x40 或 0x20 0x20）
        if not (frame[0] == 0x20 and frame[1] == 0x40):
            return False
        
        # 计算校验和（不包括最后 2 字节）
        calculated_checksum = self.ibus_checksum(frame[:-2])
        
        # 读取帧中的校验和（低字节在前）
        frame_checksum = (frame[-1] << 8) | frame[-2]
        
        return calculated_checksum == frame_checksum
    
    def parse_ibus_32byte_frame(self,frame: bytes) -> dict:
        """
        解析 32 字节的 IBus 协议帧（无数据长度字段）。
        返回值格式：
        {
            "valid": bool,           # 校验是否通过
            "channels": list[int],  # 14 个通道的 PWM 值（1000-2000）
            "checksum": {
                "calculated": int,  # 计算的校验和
                "received": int      # 接收的校验和
            }
        }
        """
        result = {
            "valid": False,
            "channels": [],
            "checksum": {
                "calculated": 0,
                "received": 0
            }
        }

        # 检查帧长度是否为 32 字节
        if len(frame) != 32:
            print(f"无效帧长度: 需要32字节，实际收到{len(frame)}字节")
            return result

        # 检查帧头 (0x20 0x40 或 0x20 0x20)
        if not (frame[0] == 0x20 and (frame[1] == 0x40 or frame[1] == 0x20)):
            print(f"无效帧头: 0x{frame[0]:02X} 0x{frame[1]:02X}")
            return result

        # 计算校验和（排除最后2字节）
        calculated_checksum = (0xFFFF - sum(frame[:-2])) & 0xFFFF
        received_checksum = (frame[-1] << 8) | frame[-2]  # 小端模式转整数

        # 提取14个通道的PWM值（小端模式）
        channels = []
        for i in range(14):
            start = 2 + i * 2  # 数据起始位置：帧头(2B) + i*2
            if start + 2 > len(frame) - 2:  # 不包含校验和
                break
            # 将小端字节转为整数（例如 [0xE8, 0x03] -> 0x03E8 = 1000）
            pwm = (frame[start + 1] << 8) | frame[start]
            channels.append(pwm)

        # 返回结果
        result["valid"] = (calculated_checksum == received_checksum)
        result["channels"] = channels
        result["checksum"]["calculated"] = calculated_checksum
        result["checksum"]["received"] = received_checksum
        return result

if __name__ == '__main__':
    mySerial = MySerial()
    
    while(True):
        print('runningserai')
        mySerial.receive_data()
        time.sleep(0.1)
